#include <SPI.h>
#include <ICM20948_WE.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <math.h>
#include <Arduino.h>
#include <Wire.h>
#include "imu.h"
// #define WIRE_PORT Wire // Your desired Wire port.      Used when "USE_SPI" is not defined
// #define AD0_VAL 0
// #define ICM20948_ADDR 0x68
// ICM20948_WE myIMU = ICM20948_WE(ICM20948_ADDR);
bool spi = true;
#define CS_PIN 14   // Chip Select Pin
ICM20948_WE myIMU = ICM20948_WE(&SPI, CS_PIN, spi);
IMU_DATA_t imu_data;
IMU_DATA_t*get_imudata_point(void)
{//获取imu相关数据指针
  return &imu_data;
}
void imu_init()
{
  // Wire.begin(17,18,100000);
   if(!myIMU.init()){
    Serial.println("ICM20948 does not respond");
  }
  else{
    Serial.println("ICM20948 is connected");
  }
   myIMU.setAccRange(ICM20948_ACC_RANGE_2G);//设置量程
   myIMU.setGyrRange(ICM20948_GYRO_RANGE_250);
   myIMU.setAccDLPF(ICM20948_DLPF_7);//设置滤波器等级
   myIMU.setGyrDLPF(ICM20948_DLPF_7); 
   myIMU.setAccSampleRateDivider(0);//设置输出速率
   myIMU.setGyrSampleRateDivider(0);
   delay(1000);
   offset_input(&imu_data);
   //autooffset(&imu_data);
   Serial.println("Done!");
}
void offset_input(IMU_DATA_t*imu_data)
{//写入IMU的零飘
  imu_data->accoffset.x=315;//130.25
  imu_data->accoffset.y=-245;//-220
  imu_data->accoffset.z=-800;//212.25
  imu_data->gyroffset.x=87.5;//166
  imu_data->gyroffset.y=12.5;//142.75
  imu_data->gyroffset.z=-125;//-37.75

  
  //  imu_data->accoffset.x=231;//130.25
  // imu_data->accoffset.y=-323;//-220
  // imu_data->accoffset.z=157;//212.25
  // imu_data->gyroffset.x=6.48;//166
  // imu_data->gyroffset.y=121.36;//142.75
  // imu_data->gyroffset.z=35.28;//-37.75
}
void autooffset(IMU_DATA_t*imu_data)
{
  xyzFloat accRaw;
  xyzFloat gyrRaw;
  /*
    这里是计算初始零飘
  */
  for(int i=0;i<1000;i++)
  {
      myIMU.readSensor();
      myIMU.getAccRawValues(&accRaw);
      myIMU.getGyrRawValues(&gyrRaw);
      imu_data->accoffset.x += accRaw.x;
      imu_data->accoffset.y += accRaw.y;
      imu_data->accoffset.z += accRaw.z;
      imu_data->gyroffset.x += gyrRaw.x;
      imu_data->gyroffset.y += gyrRaw.y;
      imu_data->gyroffset.z += gyrRaw.z;
      delay(10);
  }
    imu_data->accoffset.x /= 1000;
    imu_data->accoffset.y /= 1000;
    imu_data->accoffset.z /= 1000;
    imu_data->gyroffset.x /= 1000;
    imu_data->gyroffset.y /= 1000;
    imu_data->gyroffset.z /= 1000;
    imu_data->accoffset.y -= 16384.0;//注意这个位置默认重力加速度作用在y轴上面，减去重力加速度
    Serial.print("accoffset.x");
    Serial.print(imu_data->accoffset.x);
    Serial.print("accoffset.y");
    Serial.print(imu_data->accoffset.y );
    Serial.print("accoffset.z");
    Serial.println(imu_data->accoffset.z);
    Serial.print("gyroffset.x");
    Serial.print(imu_data->gyroffset.x);
    Serial.print("gyroffset.y");
    Serial.print(imu_data->gyroffset.y);
    Serial.print("gyroffset.z");
    Serial.println(imu_data->gyroffset.z);
}
void getaccdata(IMU_DATA_t*imu_data)
{
  /*
  输出加速度数据单位为g
  */
    xyzFloat accRaw;
     myIMU.readSensor();
     myIMU.getAccRawValues(&accRaw);
     //  Serial.print(accRaw.x);
    // Serial.print("   ");
    // Serial.print(accRaw.y);
    // Serial.print("   ");
    // Serial.println(accRaw.z);
    imu_data->accRaw.x=accRaw.x;
    imu_data->accRaw.y=accRaw.y;
    imu_data->accRaw.z=accRaw.z;
    imu_data->accdata.x=(accRaw.x-imu_data->accoffset.x)/16384;
    imu_data->accdata.y=(accRaw.y-imu_data->accoffset.y)/16384;
    imu_data->accdata.z=(accRaw.z-imu_data->accoffset.z)/16384;
}
void getgyrdata(IMU_DATA_t*imu_data)
{
  xyzFloat gyrRaw;
  /*
  输出陀螺仪数据单位为m/s2
  */
     myIMU.readSensor();
     myIMU.getGyrRawValues(&gyrRaw);
     imu_data->gyrdata.x=(gyrRaw.x-imu_data->gyroffset.x)/131;
     imu_data->gyrdata.y=(gyrRaw.y-imu_data->gyroffset.y)/131;
     imu_data->gyrdata.z=(gyrRaw.z-imu_data->gyroffset.z)/131;
    //  gyrdataVal->x=(gyrRaw.x-gyroffsetVal->x)*250/32768.0;
    //  gyrdataVal->y=(gyrRaw.y-gyroffsetVal->y)*250/32768.0;
    //  gyrdataVal->z=(gyrRaw.z-gyroffsetVal->z)*250/32768.0;
    // gyrdataVal->x=(gyrRaw.x-gyroffsetVal->x)/131;
    // gyrdataVal->y=(gyrRaw.y-gyroffsetVal->y)/131;
    // gyrdataVal->z=(gyrRaw.z-gyroffsetVal->z)/131;
}
void acctask(void *pvParameters) {
  /*
    加速度计数据获取任务，拿到加速度计数据并且解算角度
  */
  IMU_DATA_t*imu_data;
  imu_data=get_imudata_point();
    while (1) {
    // getaccdata(&accoffset,&accdata);
    // Serial.println("offset (x,y,z):");
    // Serial.print(accRaw.x);
    // Serial.print("   ");
    // Serial.print(accRaw.y);
    // Serial.print("   ");
    // Serial.println(accRaw.z);
    // Serial.println("data (x,y,z):");
    // Serial.print(accdata.x);
    // Serial.print("   ");
    // Serial.print(accdata.y);
    // Serial.print("   ");
    // Serial.println(accdata.z);
     getaccdata(imu_data);
    // Serial.println("RAWacc (x,y,z):");
    // Serial.print(imu_data->accdata.x);
    // Serial.print("   ");
    // Serial.print(imu_data->accdata.y);
    // Serial.print("   ");
    // Serial.println(imu_data->accdata.z);
    // Serial.println("RAWgyr (x,y,z):");
    // Serial.print(imu_data->gyrdata.x);
    // Serial.print("   ");
    // Serial.print(imu_data->gyrdata.y);
    // Serial.print("   ");
    // Serial.println(imu_data->gyrdata.z);
     imu_data->roll_acc = atan2(-imu_data->accdata.x, sqrt(imu_data->accdata.y*imu_data->accdata.y + imu_data->accdata.z*imu_data->accdata.z))*RAD_TO_DEG;
     imu_data->pitch_acc = atan2(-imu_data->accdata.z, sqrt(-imu_data->accdata.x*(-imu_data->accdata.x) + imu_data->accdata.y*imu_data->accdata.y))*RAD_TO_DEG;
      if(imu_data->IMU_init_flag==false)
     {//将加速度计算出的角度预存，给陀螺仪积分初始使用
      imu_data->pitch_setup_data=imu_data->pitch_acc;
      imu_data->roll_setup_data=imu_data->roll_acc;
      imu_data->IMU_init_flag=true;
     }
      // Serial.print("pitchacc");
      // Serial.println(pitch_acc);
      // Serial.print("rollacc");
      // Serial.println(roll_acc);
      vTaskDelay(1 / portTICK_PERIOD_MS);  // 延时1毫秒
    }
}
void gyrtask(void *pvParameters) {
  /*
    陀螺仪数据获取任务，拿到陀螺仪数据并且积分出角度
    右边正，左边负
    上正下负
  */
    IMU_DATA_t*imu_data;
  imu_data=get_imudata_point();
  unsigned long prev_time = millis();
    while (1) {
      unsigned long current_time =millis();
     float dt = (current_time - prev_time) / 1000.0; // 转换为秒
     prev_time = current_time;
      getgyrdata(imu_data);
      if(imu_data->IMU_init_flag==true){
        imu_data->pitch_gyro = imu_data->pitch_setup_data+imu_data->pitch +imu_data->gyrdata.x *dt;
        imu_data->roll_gyro = imu_data->roll_setup_data+imu_data->roll + imu_data->gyrdata.z * -dt;
        imu_data->yaw_gyro =imu_data->yaw+imu_data->gyrdata.y * dt;
        imu_data->pitch_setup_data=0;//初始化结束，数据清零
        imu_data->roll_setup_data=0;
      }
      // Serial.print("pitchgyr");
      // Serial.println(pitch_gyro);
      // Serial.print("rollgyr");
      // Serial.println(roll_gyro);

      vTaskDelay(1 / portTICK_PERIOD_MS); // 延时1毫秒
    }
}
void solvetask(void *pvParameters) {
  /*
    角度融合任务，实现互补滤波
  */
    IMU_DATA_t*imu_data;
  imu_data=get_imudata_point();
  unsigned long prev_time = millis();
    while (1) {
       imu_data->delay_speed_time++;
      
       unsigned long current_time =millis();
     float dt = (current_time - prev_time) / 1000.0; // 转换为秒
     prev_time = current_time;
      imu_data->pitch = imu_data->alpha * imu_data->pitch_gyro + (1 - imu_data->alpha) * imu_data->pitch_acc;
      imu_data->roll = imu_data->alpha * imu_data->roll_gyro + (1 - imu_data->alpha) * imu_data->roll_acc;
      imu_data->yaw =imu_data->yaw_gyro;//yaw暂时没有融合算法，先单放着
      imu_data->sin_pitch = sin(imu_data->pitch*DEG_TO_RAD);
      imu_data->cos_pitch = cos(imu_data->pitch*DEG_TO_RAD);
      imu_data->sin_roll = sin(imu_data->roll*DEG_TO_RAD);
      imu_data->cos_roll = cos(imu_data->roll*DEG_TO_RAD);
      imu_data->aircraft_att[0]=-imu_data->sin_pitch;
      imu_data->aircraft_att[1]=imu_data->sin_roll*imu_data->cos_pitch;
      imu_data->aircraft_att[2]=imu_data->cos_roll*imu_data->cos_pitch;
      imu_data->no_filteraccz=((imu_data->accdata.z*imu_data->aircraft_att[0]-imu_data->accdata.x*imu_data->aircraft_att[1]+imu_data->accdata.y*imu_data->aircraft_att[2])-1)*9.81;
      if(imu_data->delay_speed_time>1000)
      {
       imu_data->speed_z=imu_data->speed_z+imu_data->no_filteraccz*dt;
     
       if(abs(imu_data->no_filteraccz)<0.02) 
       {//零速重置死区
        imu_data->speed_z=0;
       }
       imu_data->high+=imu_data->speed_z*dt;
      }
      
      //Serial.println(10);
      //  Serial.print("pitch:");
      // Serial.println(imu_data->pitch);
      // Serial.println(50);
      // Serial.print("roll:");
      // Serial.println(imu_data->roll);
      // Serial.println(-50);
      // Serial.print("yaw:");
      // Serial.println(imu_data->yaw);
      // Serial.println(5);
      // Serial.print("speedz:");
      // Serial.println(imu_data->speed_z);
    //   Serial.println(5);
    //     Serial.print("speedz:");
    //     Serial.println(imu_data->speed_z);
     Serial.println(5);
      Serial.print("high:");
      Serial.println(imu_data->high);
      Serial.println(-5);

      // Serial.print("dt");
      // Serial.println(dt*1000);
       vTaskDelay(1/ portTICK_PERIOD_MS);  // 延时1毫秒
    }
}